#include "vio/front_end/frame_handler_base.h"

namespace vio {
FrameHandlerBase::FrameHandlerBase()
    : stage_(STAGE_PAUSED),
      set_reset_(false),
      set_start_(false),
      acc_frame_timings_(10),
      acc_num_obs_(10),
      num_obs_last_(0),
      tracking_quality_(TRACKING_INSUFFICIENT) {
  LOG(INFO) << LOG_GREEN_BEGIN << "VIO inited!" << LOG_COLOR_END;
}

bool FrameHandlerBase::StartFrameProcessingCommon(const double timestamp) {
  if (set_start_) {
    ResetAll();
    stage_ = STAGE_FIRST_FRAME;
  }
  if (stage_ == STAGE_PAUSED) {
    return false;
  }
  LOG(INFO) << LOG_GREEN_BEGIN << "total time" << LOG_COLOR_END;
  timer_.start();

  // some cleanup from last iteration, can't do before because of visualization
  map_.EmptyTrash();
  return true;
}

int FrameHandlerBase::FinishFrameProcessingCommon(
    const size_t update_id,
    const UpdateResult dropout,
    const size_t num_observations) {
  LOG(INFO) << "Frame: " << update_id
            << "\t fps-avg = " << 1.0 / acc_frame_timings_.getMean()
            << "\t nObs = " << acc_num_obs_.getMean();
  LOG(INFO) << dropout;

  // save processing time to calculate fps
  acc_frame_timings_.push_back(timer_.stop());
  if (stage_ == STAGE_DEFAULT_FRAME) {
    acc_num_obs_.push_back(num_observations);
  }
  num_obs_last_ = num_observations;

  if (dropout == RESULT_FAILURE &&
      (stage_ == STAGE_DEFAULT_FRAME || stage_ == STAGE_RELOCALIZATION)) {
    stage_ = STAGE_RELOCALIZATION;
    tracking_quality_ = TRACKING_INSUFFICIENT;
  } else if (dropout == RESULT_FAILURE) {
    ResetAll();
  }
  if (set_reset_) {
    ResetAll();
  }
  return 0;
}

void FrameHandlerBase::ResetCommon() {
  map_.Reset();
  stage_ = STAGE_PAUSED;
  set_reset_ = false;
  set_start_ = false;
  tracking_quality_ = TRACKING_INSUFFICIENT;
  num_obs_last_ = 0;
  LOG(INFO) << LOG_RED_BEGIN << "VIO reset" << LOG_COLOR_END;
}

void FrameHandlerBase::SetTrackingQuality(const size_t num_observations) {
  tracking_quality_ = TRACKING_GOOD;
  if (num_observations < 50) {
    LOG(INFO) << "Tracking less than 50 features";
    tracking_quality_ = TRACKING_INSUFFICIENT;
  }
  const int feature_drop =
      static_cast<int>(std::min(num_obs_last_, size_t(120))) - num_observations;
  if (feature_drop > 40) {
    LOG(INFO) << "Lost " << feature_drop << " features!";
    tracking_quality_ = TRACKING_INSUFFICIENT;
  }
}

bool PtLastOptimComparator(Point::Ptr lhs, Point::Ptr rhs) {
  return (lhs->last_structure_optim_ts_ < rhs->last_structure_optim_ts_);
}

void FrameHandlerBase::OptimizeStructure(Frame::Ptr frame,
                                         size_t max_n_pts,
                                         int max_iter) {
  std::deque<Point::Ptr> pts;
  // frame all featuress points
  for (Features::iterator it = frame->features_.begin();
       it != frame->features_.end();
       ++it) {
    auto pt_ptr = (*it)->point_ptr.lock();
    if (pt_ptr) {
      pts.push_back(pt_ptr);
    }
  }
  max_n_pts = std::min(max_n_pts, pts.size());
  std::nth_element(
      pts.begin(), pts.begin() + max_n_pts, pts.end(), PtLastOptimComparator);
  for (std::deque<Point::Ptr>::iterator it = pts.begin();
       it != pts.begin() + max_n_pts;
       ++it) {
    (*it)->Optimize(max_iter);
    (*it)->last_structure_optim_ts_ = frame->id_;
  }
}

}  // namespace vio
